#include <buola/buola.h>

#ifdef BUOLA_USE_INTRINSICS
#include <xmmintrin.h>
#include <emmintrin.h>
#include <pmmintrin.h>
#endif

#include <buola/image/algorithm/filter.h>
#include <buola/image/algorithm/arithmetic.h>
#include <buola/image/algorithm/statistics.h>
#include <buola/cv/cattention.h>

namespace buola { namespace cvi {

CAttention::CAttention()
    :   mWeights(WEIGHT_COUNT,1.0),
        mConcentration(0.5),
        mAttention(0.5),
        mKoeff(0.0)
{
}

CAttention::~CAttention()
{

}

void CAttention::CalcColourMaps(const img::CView_rgb8 &pSrc)
{
    mRed.Recreate(pSrc.Size());
    mGreen.Recreate(pSrc.Size());
    mBlue.Recreate(pSrc.Size());
    mYellow.Recreate(pSrc.Size());
    mIntensity.Recreate(pSrc.Size());

    //Broadly tuned color channels
    float *lR=mRed.RawData();
    float *lG=mGreen.RawData();
    float *lB=mBlue.RawData();
    float *lY=mYellow.RawData();
    float *lI=mIntensity.RawData();

    int lSize=pSrc.Width()*pSrc.Height();
    const unsigned char * lSrc=pSrc.RawData();

#ifdef BUOLA_USE_INTRINSICS
    __m128 l3=_mm_set1_ps(3.0f);

    for(int i=0;i<lSize;i+=4)
    {
        __m128 r=_mm_setr_ps(lSrc[i*3+0],lSrc[i*3+3],lSrc[i*3+6],lSrc[i*3+9]);
        __m128 g=_mm_setr_ps(lSrc[i*3+1],lSrc[i*3+4],lSrc[i*3+7],lSrc[i*3+10]);
        __m128 b=_mm_setr_ps(lSrc[i*3+2],lSrc[i*3+5],lSrc[i*3+8],lSrc[i*3+11]);

        __m128 d1=_mm_sub_ps(r,g);
        __m128 d2=_mm_sub_ps(g,b);
        __m128 d3=_mm_sub_ps(b,r);

        _mm_store_ps(lR+i,_mm_sub_ps(d1,d3));
        _mm_store_ps(lG+i,_mm_sub_ps(d2,d1));
        _mm_store_ps(lB+i,_mm_sub_ps(d3,d2));
        _mm_store_ps(lY+i,_mm_sub_ps(_mm_min_ps(r,g),b));
        _mm_store_ps(lI+i,_mm_div_ps(_mm_add_ps(r,_mm_add_ps(g,b)),l3));
    }
#else
    for (int i=0;i<lSize;i++)
    {
        float r=lSrc[i*3+0];
        float g=lSrc[i*3+1];
        float b=lSrc[i*3+2];

        lR[i]=2*r-(g + b);
        lG[i]=2*g-(r + b);
        lB[i]=2*b-(r + g);
        lY[i]=std::min(r,g)-b;
        lI[i]=(r+g+b)/3.0f;
    }
#endif
/*
    for (int i=0;i<lSize;i++)
    {
        float r=lSrc[i*3+0];
        float g=lSrc[i*3+1];
        float b=lSrc[i*3+2];

        assert(lR[i]==2*r-(g + b));
        assert(lG[i]==2*g-(r + b));
        assert(lB[i]==2*b-(r + g));
        assert(lY[i]==std::min(r,g)-b);
        assert(lI[i]==(r+g+b)/3.0f);
    }
*/
}

void CAttention::CalcOrientation(const img::CView_rgb8 &pL)
{
    // Orientation maps
    img::CImage_gray32f *lOImgs[4];

    for(int i=0;i<4;i++)
        lOImgs[i]=new img::CImage_gray32f(pL.Size());

    gabor_filter(mIntensity,*lOImgs[0],*lOImgs[1],*lOImgs[2],*lOImgs[3]);

    mOConsp.Recreate(pL.Size()/2);

    img::CImage_gray32f lAngConsp(pL.Size()/2);

    for(int i=0;i<4;i++)
    {
        mTmpPyramid.Recreate(*lOImgs[i],8);
        CSD(mTmpPyramid,mTmpCSD);
        if(i==0)
        {
            MakeConspicuityMaps(mTmpCSD,mOConsp,true);
            gaussian_blur(mOConsp);
            Normalize(mOConsp);
        }
        else
        {
            MakeConspicuityMaps(mTmpCSD,lAngConsp,true);
            gaussian_blur(lAngConsp);
            Normalize(lAngConsp);
            accumulate(lAngConsp,mOConsp);
        }

        delete lOImgs[i];
    }
}

void CAttention::Normalize(const img::CMView_gray32f &pI)
{
    //this should actually be to the power of mAlpha, but since it is 1.5, use faster alternative
    pow1p5(pI);

    //find min and max in image
    float lMin,lMax;
    image_minmax(pI,lMin,lMax);

    //find mean of local maxima
    float lMean=local_max_mean(pI, lMax);

    float lFac = pow(1.0f - lMean/lMax,mBeta)/(lMax-lMin);
    add_and_scale(pI,-lMin,lFac);
}

void CAttention::CSD(tPyramid &pSrc,tPyramid &pCSD)
{
    pCSD.clear();

    for (int c=mHighScale;c<=mLowScale;c++)
    {
        const img::CView_gray32f &lSrc=pSrc(c);

        img::CImage_gray32f lTmp(lSrc.Size());

        for (int d=3; d<=4; d++)
        {
            img::CImage_gray32f* lNew=new img::CImage_gray32f(lSrc.Size());

            resize(pSrc(c+d),lTmp);
            abs_diff(lSrc,lTmp,*lNew);
            pCSD.push_back(lNew);
        }
    }
}

void CAttention::MakeConspicuityMaps(tPyramid &pCSD,const img::CMView_gray32f &pConsp,bool pClear)
{
    img::CImage_gray32f lTmp(pConsp.Size());

    for (int i=0;i<pCSD.NumLevels();i++)
    {
        Normalize(pCSD(i));

        if(pClear&&i==0) //clear the conspicuity map
        {
            resize(pCSD(i),pConsp);
        }
        else
        {
            resize(pCSD(i),lTmp);
            accumulate(lTmp,pConsp);
        }
    }
}

void CAttention::CalcSM_BU(const img::CView_rgb8 &pL)
{
    // Colour maps
    img::CImage_gray32f lCConsp(mSM_BU.Size());

    img::CImage_gray32f lColDiff(pL.Size());
    sub(mRed,mGreen,lColDiff);
    tPyramid lRGPyr(lColDiff,8),lCSD_RGPyr;
    CSD(lRGPyr,lCSD_RGPyr);
    MakeConspicuityMaps(lCSD_RGPyr,lCConsp,true);

    sub(mBlue,mYellow,lColDiff);
    tPyramid lBYPyr(lColDiff,8),lCSD_BYPyr;
    CSD(lBYPyr,lCSD_BYPyr);
    MakeConspicuityMaps(lCSD_BYPyr,lCConsp,false);

    copy_pixels(mIConsp,mSM_BU);
    accumulate(lCConsp,mSM_BU);
    accumulate(mOConsp,mSM_BU);
}

void CAttention::CalcSM_TD(const img::CView_rgb8 &pL)
{
    // Colour maps
    img::CImage_gray32f lCConsp(mSM_BU.Size(),img::pixel(0.0f));

    img::CImage_gray32f lColDiff(pL.Size());

    add_weighted(mRed,mWeights[WEIGHT_R],mGreen,-mWeights[WEIGHT_G],lColDiff);
    tPyramid lRGPyr(lColDiff,8),lCSD_RGPyr;
    CSD(lRGPyr,lCSD_RGPyr);
    MakeConspicuityMaps(lCSD_RGPyr,lCConsp,true);

    add_weighted(mBlue,mWeights[WEIGHT_B],mYellow,-mWeights[WEIGHT_Y],lColDiff);
    tPyramid lBYPyr(lColDiff,8),lCSD_BYPyr;
    CSD(lBYPyr,lCSD_BYPyr);
    MakeConspicuityMaps(lCSD_BYPyr,lCConsp,false);

    add_weighted(mIConsp,mWeights[WEIGHT_INTENSITY],lCConsp,mWeights[WEIGHT_COLOR],mSM_TD);

    accumulate(mOConsp,mSM_TD);
}

void CAttention::CombineSM(const img::CMView_gray32f &SM, const img::CView_gray32f &SM_BU, const img::CView_gray32f &SM_TD)
{
    float E_BU = CSI(SM_BU);
    float E_TD = CSI(SM_TD);
    //float dk = -mConcentration*k+mAttention*sqrt(2*E_BU/E_TD);
    float dk = 0;

    if(E_TD!=0)
        dk = -mConcentration*mKoeff+mAttention*(E_BU/E_TD);

    mKoeff += dk;

    if (mKoeff>1)
        mKoeff=1;
    else if (mKoeff<0)
        mKoeff=0;

    msg_info() << "mKoeff=" << mKoeff << "\n";
    add_weighted(SM_BU,mKoeff,SM_TD,1-mKoeff,SM);
//    msg_info() << "k-value = " << mKoeff << "\n";
}

float CAttention::CSI(const img::CView_gray32f &pMap)
                                      //composite saliency indicator
{
    float thresh = MaxEnThr(pMap);

    if(thresh==0)
        return 0.0;

    msg_info() << "not implemented thing\n";
    return 0.0;
/*
    int w = smap.Width();
    int h = smap.Height();
    const img::CView_gray32f smapthr(w,h);
    Image<bool> salmask(w,h);
    float *sald = smap.GetData();
    float *sthrd = smapthr.GetData();
    bool *smaskd = salmask.GetData();
    for(int i=0; i<w*h; i++)
      if(sald[i]>thresh)
    {
      sthrd[i]=i;
      smaskd[i]=true;
    }
      else
    {
      sthrd[i]=0;
      smaskd[i]=false;
    }
    float density = saldens(smapthr);
    float hullarea = convhull(salmask);
    return (density/hullarea);
*/
}

#if 0
void num_pix(const img::CView_gray32f &smap, std::vector<float> &thvals,
                    float thrstep, std::vector<float> &output)
            //normalized number of pixels with certain intensity values
{
  int w = smap.Width();
  int h = smap.Height();
  float *sald = smap.GetData();
  int n=thvals.size();
  int sum = 0;

  for(int i=0; i<w*h; i++)
    for(int j=0; j<n; j++)
      if(abs(thvals[j]-sald[i])<thrstep)
    {
      ++output[j];
      ++sum;
    }

  for(int j=0; j<n; j++)
    output[j]=output[j]/sum;

}


int argmin_vec(std::vector<float> &vec)  //index of the minimum element of a vector
{
  int n = vec.size();
  float min=1000000;
  int indx;
  for(int i=0; i<n; i++)
    if(vec[i]<min)
      {
    min=vec[i];
    indx=i;
      }
  return indx;
}
#endif

float CAttention::MaxEnThr(const img::CView_gray32f &pMap)  //maximum entropy threshold
{
    float m,M;
    min_max(pMap,m,M);
    int N=200;

    int thrstep = (int)round((M-m)/N);     //choose resolution of 200 for thresholding steps

    if(thrstep==0)
        return 0.0;

    msg_info() << "not implemented thing!!!\n";
    return 0.0;

/*
    std::vector<float> func(N); //possible bug in original
    for(int l=1; l<N; l++)
    {
        std::vector<float> thvals1(l);
        for(int i=0; i<l; i++)
            thvals1[i]=thrstep*(i+1);
        std::vector<float> p1_norm(l);
        num_pix(smap, thvals1, thrstep/2, p1_norm);
        float temp1=0;
        for(int i=0; i<p1_norm.size(); i++)
            if(p1_norm[i]!=0)
                temp1 += p1_norm[i]*log(p1_norm[i]);

        std::vector<float> thvals2(N-l);
        for(int i=0; i<N-l; i++)
            thvals2[i]=thrstep*(i+l+1);
        std::vector<float> p2_norm(N-l);
        num_pix(smap, thvals2, thrstep/2, p2_norm);
        float temp2=0;
        for(int i=0; i<p2_norm.size(); i++)
            if(p2_norm[i]!=0)
                temp2 += p2_norm[i]*log(p2_norm[i]);

        msg_info() << temp1 << " " << temp2 << "\n";
        func[l]=temp1+temp2;
    }

    return thrstep*argmin_vec(func);
*/
}


#if 0

struct Point{
  int x, y;
};

float CAttention::saldens(const img::CView_gray32f &smap)   //saliency density
{
  int w = smap.Width();
  int h = smap.Height();
  float *sald = smap.GetData();
  float totalsal=0;
  float totalnum=0;

  for(int y=1; y<h-1; y++)
    for(int x=1; x<w-1; x++)
      {
    float sm=sald[y*w+x];
    if(sm>0)
      {
        float temp=0;
        float numsal=0;
        for(int iy=-1; iy<1; iy++)
          for(int ix=-1; ix<1; ix++)
        {
          float smi=sald[(y+iy)*w+x+ix];
          if(sm>0)
            {
              temp += abs(sm-smi);
              ++numsal;
            }
          temp = temp/(numsal-1);
        }
        ++totalnum;
        totalsal += temp;
      }
      }
     return totalsal/totalnum;

}
#endif

#if 0

// Assume that classes are already given for the objects:
//    Point with coordinates {float x, y;}
//===================================================================

// isLeft(): tests if a point is Left|On|Right of an infinite line.
//    Input:  three points P0, P1, and P2
//    Return: >0 for P2 left of the line through P0 and P1
//            =0 for P2 on the line
//            <0 for P2 right of the line
//    See: the January 2001 Algorithm on Area of Triangles
inline float isLeft( Point P0, Point P1, Point P2 )
{
    return (P1.x - P0.x)*(P2.y - P0.y) - (P2.x - P0.x)*(P1.y - P0.y);
}
//===================================================================


#define NONEx  (-1)

typedef struct range_bin Bin;
struct range_bin {
    int    min;    // index of min point P[] in bin (>=0 or NONEx)
    int    max;    // index of max point P[] in bin (>=0 or NONEx)
};


// nearHull_2D(): the BFP fast approximate 2D convex hull algorithm
//     Input:  P[] = an (unsorted) array of 2D points
//              n = the number of points in P[]
//              k = the approximation accuracy (large k = more accurate)
//     Output: H[] = an array of the convex hull vertices (max is n)
//     Return: the number of points in H[]
int nearHull_2D( Point* P, int n, int k, Point* H )
{
    int    minmin=0,  minmax=0;
    int    maxmin=0,  maxmax=0;
    float  xmin = P[0].x,  xmax = P[0].x;
    Point* cP;                 // the current point being considered
    int    bot=0, top=(-1);  // indices for bottom and top of the stack

    // Get the points with (1) min-max x-coord, and (2) min-max y-coord
    for (int i=1; i<n; i++) {
        cP = &P[i];
        if (cP->x <= xmin) {
            if (cP->x < xmin) {        // new xmin
                xmin = cP->x;
                minminl = minmax = i;
            }
            else {                      // another xmin
                if (cP->y < P[minmin].y)
                    minmin = i;
                else if (cP->y > P[minmax].y)
                    minmax = i;
            }
        }
        if (cP->x >= xmax) {
            if (cP->x > xmax) {        // new xmax
                xmax = cP->x;
                maxmin = maxmax = i;
            }
            else {                      // another xmax
                if (cP->y < P[maxmin].y)
                    maxmin = i;
                else if (cP->y > P[maxmax].y)
                    maxmax = i;
            }
        }
    }
    if (xmin == xmax) {      // degenerate case: all x-coords == xmin
        H[++top] = P[minmin];           // a point, or
        if (minmax != minmin)           // a nontrivial segment
            H[++top] = P[minmax];
        return top+1;                   // one or two points
    }

    // Next, get the max and min points in the k range bins
    Bin*   B = new Bin[k+2];   // first allocate the bins
    B[0].min = minmin;         B[0].max = minmax;        // set bin 0
    B[k+1].min = maxmin;       B[k+1].max = maxmax;      // set bin k+1
    for (int b=1; b<=k; b++) { // initially nothing is in the other bins
        B[b].min = B[b].max = NONEx;
    }
    for (int b, i=0; i<n; i++) {
        cP = &P[i];
        if (cP->x == xmin || cP->x == xmax) // already have bins 0 and k+1
            continue;
        // check if a lower or upper point
        if (isLeft( P[minmin], P[maxmin], *cP) < 0) {  // below lower line
            b = (int)( k * (cP->x - xmin) / (xmax - xmin) ) + 1;  // bin #
        Point *cP2 = &P[B[b].min];
        if (B[b].min == NONEx)       // no min point in this range
                B[b].min = i;           // first min
            else if (cP->y < cP2->y)
                B[b].min = i;           // new min
            continue;
        }
        if (isLeft( P[minmax], P[maxmax], *cP) > 0) {  // above upper line
            b = (int)( k * (cP->x - xmin) / (xmax - xmin) ) + 1;  // bin #
        Point *cP2 = &P[B[b].min];
            if (B[b].max == NONEx)       // no max point in this range
                B[b].max = i;           // first max
            else if (cP->y > cP2->y)  //why not P[B[b].min]->y
                B[b].max = i;           // new max
            continue;
        }
    }

    // Now, use the chain algorithm to get the lower and upper hulls
    // the output array H[] will be used as the stack
    // First, compute the lower hull on the stack H
    for (int i=0; i <= k+1; ++i)
    {
        if (B[i].min == NONEx)  // no min point in this range
            continue;
        cP = &P[ B[i].min ];   // select the current min point

        while (top > 0)        // there are at least 2 points on the stack
        {
            // test if current point is left of the line at the stack top
            if (isLeft( H[top-1], H[top], *cP) > 0)
                break;         // cP is a new hull vertex
            else
                top--;         // pop top point off stack
        }
        H[++top] = *cP;        // push current point onto stack
    }

    // Next, compute the upper hull on the stack H above the bottom hull
    if (maxmax != maxmin)      // if distinct xmax points
        H[++top] = P[maxmax];  // push maxmax point onto stack
    bot = top;                 // the bottom point of the upper hull stack
    for (int i=k; i >= 0; --i)
    {
        if (B[i].max == NONEx)  // no max point in this range
            continue;
        cP = &P[ B[i].max ];   // select the current max point

        while (top > bot)      // at least 2 points on the upper stack
        {
            // test if current point is left of the line at the stack top
            if (isLeft( H[top-1], H[top], *cP) > 0)
                break;         // current point is a new hull vertex
            else
                top--;         // pop top point off stack
        }
        H[++top] = *cP;        // push current point onto stack
    }
    if (minmax != minmin)
        H[++top] = P[minmin];  // push joining endpoint onto stack

    delete B;                  // free bins before returning
    return top+1;              // # of points on the stack
}


float CAttention::convhull(Image<bool> &mask)  //area of convex hull
{
  int w = mask.Width();
  int h = mask.Height();
  bool *maskd = mask.GetData();
  Point *P;
  Point *H;
  int i=0;
  for(int y=0; y<h; y++)
    for(int x=0; x<w; x++)
      if(mask[y*w+x])
    {
      P[i].x=x;
      P[i].y=y;
      ++i;
    }
  int K = nearHull_2D(P, i, 100, H);
  float hullsize=0;
  for(int k=0; k<K-1; k++)
    {
      Point cH1 = H[k];
      Point cH2 = H[k+1];
      hullsize += (cH2.y+cH1.y)*(cH2.x-cH1.x)/2;
    }

  return hullsize;
}

#endif


float CAttention::mean_locmax(const img::CView_gray32f &pImage, float maximum, int xsize)
{
  const float *imd = pImage.RawData();
  int h = pImage.Height();
  int w = pImage.Width();;
  int resol = w/xsize;
  int ysize = h/resol;
  float sumMax = 0.0f;
  for (int j=0;j<ysize;j++) {
    for (int i=0;i<xsize;i++) {
      int yp = j*resol;
      int xp = i*resol;
      int p = yp*w + xp;
      float maxVal = imd[p];
      for (int y=0;y<resol;y++)
    for (int x=0;x<resol;x++)
      if (imd[p+y*w+x]>maxVal)
        maxVal = imd[p+y*w+x];
      sumMax += maxVal;
    }
  }
  float mean = (sumMax - maximum) / (ysize*xsize - 1);
  return mean;
}

CAttention::SPeak CAttention::FindMaxPeak(const img::CView_gray32f &pMap)
{
    const float *lData = pMap.RawData();
    int w=pMap.Width();
    int h=pMap.Height();

    float lMax=lData[0];
    int lMaxPos=0;
    for(int i=1; i<h*w; i++)
    {
        if(lData[i]>lMax)
        {
            lMax=lData[i];
            lMaxPos=i;
        }
    }

    return SPeak(lMaxPos%w,lMaxPos/w,lMax,0);
}

void CAttention::IOR(const img::CMView_gray32f &pMap,const SPeak &pPeak, float k)
{
    float *lData = pMap.RawData();
    int w=pMap.Width();
    int h=pMap.Height();
    float px=pPeak.mPos.x;
    float py=pPeak.mPos.y;
    float t=1.0;
    int xmin=std::max((int)px-30,0);
    int xmax=std::min((int)px+30,w);
    int ymin=std::max((int)py-30,0);
    int ymax=std::min((int)py+30,h);

    for(int y=ymin; y<ymax; y++)
        for(int x=xmin; x<xmax; x++)
        {
            int i=y*w+x;
            lData[i]=lData[i]*exp(-k*lData[i]*t/
                    (sqrt((x-px)*(x-px)+(y-py)*(y-py))+1));
        }
}

void CAttention::FindPeaks(const img::CView_gray32f &pMap, int pN)
{
    int w=pMap.Width();
    int h=pMap.Height();
    img::CImage_gray32f lTmp(pMap.Size());

    copy_pixels(pMap,lTmp);

    mPeaks.clear();

    do
    {
        SPeak lPeak=FindMaxPeak(lTmp); //find peak
        if(lPeak.mPos.x>8&&lPeak.mPos.y>8&&lPeak.mPos.x<w-8&&lPeak.mPos.y<h-8)
            mPeaks.push_back(lPeak); //and push into peaks
        IOR(lTmp,lPeak,mIORRate); //inhibition of return one time-step
    } while(mPeaks.size()<pN);
}

void CAttention::Update(const img::CView_rgb8 &pI)
{

    CSize_i lSize(pI.Width(),pI.Height());
    int lScale=(1<<mHighScale);
    CSize_i lSMSize(lSize/lScale);

    CalcColourMaps(pI);

    mTmpPyramid.Recreate(mIntensity,8);
    CSD(mTmpPyramid,mTmpCSD);
    mIConsp.Recreate(lSMSize);
    MakeConspicuityMaps(mTmpCSD,mIConsp,true);

    CalcOrientation(pI);

    mSM.Recreate(lSMSize);
    mSM_BU.Recreate(lSMSize);
    mSM_TD.Recreate(lSMSize);

    CalcSM_BU(pI);
    CalcSM_TD(pI);
    CombineSM(mSM,mSM_BU,mSM_TD);

    FindPeaks(mSM,mNumPeaks);
}

/*namespace cvi*/ } /*namespace buola*/ }
